#include <stdio.h>
#include <string.h>
#include <avr/io.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include "uart.h"
#include "gps.h"
#include "defines.h"
#include "pos.h"

char GPSstring[256] = {0};
uint8_t GPSc = 0;

void SetupGPS(void)
{
	uart_init(4800);
}

void GPSpoll(void)
{
	if(uart_available())
	{
		char i = uart_getchar();
		
		if(i == '$')
		{
			GPSstring[GPSc] = '\0';
			HandleString(GPSstring);
			GPSc = 0;
		}

		GPSstring[GPSc] = i;
		GPSc++;


	}
}

void HandleString(char* b)
{
	uint8_t type = 0;
	uint8_t r;

	
	if(memcmp(b, "$GPGGA,", 7) == 0)
		type = 1;

	if(memcmp(b, "$GPRMC,", 7) == 0)
		type = 2;

	if(type == 0)
		return;

	r = HandleChecksum(b);

	if(r == 1)//bad checksum
		return;

	if(type == 1)
		HandleGGA(b);
	
	if(type == 2)
		HandleRMC(b);
}


void HandleGGA(char* b)//lat, long, fix, alt
{
	float lat;
	char latp;
	float lon;
	char lonp;
	char fix;
	int16_t alt;
	char * r;


	r = strtok(b, ",");//$GPGGA
	r = strtok(NULL, ",");//timestamp

	r = strtok(NULL, ",");//latitude
	lat = atof(r);

	r = strtok(NULL, ",");//N or S
	latp = *r;

	r = strtok(NULL, ",");//longitude
	lon = atof(r);

	r = strtok(NULL, ",");//E or W
	lonp = *r;

	r = strtok(NULL, ",");//fix
	fix = *r;
	

	r = strtok(NULL, ",");//satellites
	r = strtok(NULL, ",");//HDOP

	r = strtok(NULL, ",");//altitude
	alt = atof(r);

	SetLatitude(lat, latp);
	SetLongitude(lon, lonp);
	SetAlt(alt);
	SendPacket();
}

void HandleRMC(char* b)
{
	uint16_t speed;
	uint16_t heading;
	char* r;
	char v;

	r = strtok(b, ",");//$GPRMC
	r = strtok(NULL, ",");//timestamp
	r = strtok(NULL, ",");//validity
	v = *r;
	SetValidity(v);
	r = strtok(NULL, ",");//lat
	r = strtok(NULL, ",");//n or s
	r = strtok(NULL, ",");//lon
	r = strtok(NULL, ",");//e or w

	r = strtok(NULL, ",");//speed
	speed = (uint16_t)atoi(r);

	r = strtok(NULL, ",");//heading
	heading = (uint16_t)atoi(r);	


	SetHeading(heading);
	SetSpeed(speed);
}

uint8_t HandleChecksum(char* b)
{
	volatile uint8_t chksum;
	volatile uint8_t i = 0;
	char chkbuf[3] = {0};

	chksum = b[1];
	i = 2;//start past the '$'
	while(b[i] != '*')
	{
		chksum ^= b[i];
		i++;
	}
	i++;//get past the '*'
	
	itoa(chksum, chkbuf, 16);
	
	for(uint8_t o = 0; o < 3; o++)
		chkbuf[o] = toupper(chkbuf[o]);
	
	if(memcmp(b+i,chkbuf,2) == 0)
		return 0;
	else
		return 1;
}
